{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

#pragma once

#include <Eigen/Dense>

#include "../group_ops.h"

namespace sym {

/**
 * C++ GroupOps implementation for matrices.
 */
template <typename ScalarType, int Rows, int Cols>
struct GroupOps<Eigen::Matrix<ScalarType, Rows, Cols>> {
  using Scalar = ScalarType;
  using T = Eigen::Matrix<Scalar, Rows, Cols>;
  static_assert(std::is_floating_point<ScalarType>::value, "");

  static T Identity() {
    return T::Zero();
  }

  static T Inverse(const T& a) {
    return -a;
  }

  static T Compose(const T& a, const T& b) {
    return b + a;
  }

  static T Between(const T& a, const T& b) {
    return b - a;
  }
};

}  // namespace sym

// Explicit instantiation
{% for scalar in scalar_types %}
{% for i in range(1, 10) %}
extern template struct sym::GroupOps<Eigen::Matrix<{{ scalar }}, {{ i }}, 1>>;
{% endfor %}
{% for i in range(2, 10) %}
extern template struct sym::GroupOps<Eigen::Matrix<{{ scalar }}, {{ i }}, {{ i }}>>;
{% endfor %}
{% endfor %}
